/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.templates.commands.ManualArmDrive;

/**
 *
 * @author Jarrod Risley
 */
public class PIDMainArmSubsystem extends PIDSubsystem {

    /*
     * Global Declarations
     */
    Victor mainArmMotor;
    AnalogChannel mainArmPot;
    
    /*
     * These are the PID constants. They need to be tuned AFTER we select
     * our predetermined positions.
     */
    
    private static final double Kp = 0.0;
    private static final double Ki = 0.0;
    private static final double Kd = 0.0;
    
    private double lastMoveTime = 0.0;
    private double timeToTraverseACount = 0.015;
    private double minImpulseDuration = 0.5;
    private double controlImpulseDuration = 3.0;
    private boolean victorEnabled = false;
    private String armName;
    
    /**
     * These values are for the base arm unit - a-k-a the part of the arm
     * attached to the arm tower.
     */
    
    public static class MainArmPosition {
        
        /*
         * Local Variables
         * 
         * ...These need to be calibrated.
         */
        
        
        private double value;
        private static final double EXTENDED = 0;
        private static final double FULLY_LOWERED = 0;
        private static final double INTERMEDIATE_A = 0;
        private static final double INTERMEDIATE_B = 0;
        
        public static final MainArmPosition MAIN_ARM_EXTENDED = 
                new MainArmPosition(EXTENDED);
        
        public static final MainArmPosition MAIN_ARM_FULLY_LOWERED =
                new MainArmPosition(FULLY_LOWERED);
        
        public static final MainArmPosition MAIN_ARM_INTERMEDIATE_A =
                new MainArmPosition(INTERMEDIATE_A);
        
        public static final MainArmPosition MAIN_ARM_INTERMEDIATE_B = 
                new MainArmPosition(INTERMEDIATE_B);
        
        
        /**
         * Constructs a MainArmPosition - a specified potentiometer value.
         * 
         * @param position The potentiometer value that corresponds to a 
         * desired height for the arm.
         */
        
        private MainArmPosition(double position) {
            this.value = position;
        } // end MainArmPosition Constructor
        
    } // end MainArmPosition
    
    // METHODS //
    
    /**
     * Sets the default command for this subsystem.
     */
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        
        /*
         * Gives the Co-driver control of the main arm.
         */
        
        setDefaultCommand(new ManualArmDrive());
        
    } // end initDefaultCommand
    
    /**
     * Grabs the reading from the main arm potentiometer.
     * 
     * @return The current value of the potentiometer.
     */
    
    public double getMainArmPotValue() {
        return returnPIDInput();
    } // end getMainArmPosition
    
    /**
     * Does the slog work for the method above.
     * 
     * @return The current value of the potentiometer.
     */
    
    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;
        
        double position = mainArmPot.getAverageValue();
        
        return position;
    } // end returnPIDInput
    
    /**
     * Checks to see if the victor / motor has been enabled.
     * 
     * @return True if enabled.
     */
    
    public boolean isMotorEnabled() {
        return victorEnabled;
    } // end isMotorEnabled
    
    /**
     * Drive the whole arm unit to the specified position.
     * 
     * @param output The predetermined position.
     */
    
    public void setMainArmPosition(MainArmPosition position) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
        
        enable();
        victorEnabled = true;
        controlImpulseDuration = minImpulseDuration + timeToTraverseACount
                * Math.abs(position.value - getMainArmPotValue());
        lastMoveTime = Timer.getFPGATimestamp();
        setSetpoint(position.value);
        
    } // end setMainArmPosition
    
    /**
     * Not quite sure how this is implemented in superclasses...
     * 
     * @param output IDK...?
     */
    
    protected void usePIDOutput(double output) {
    
        mainArmMotor.set(output);
        
        if ((victorEnabled)
                && (Timer.getFPGATimestamp() > (lastMoveTime + controlImpulseDuration))) {
            // NOTE: This order is important because disable()ing the PID controller stops immediately, it does
            //       not allow the function to finish
            //System.out.println("Disabling Lifter Motor " + lifterName + " " + m_Lifter + " @ " + lastEnableCommand);
            victorEnabled = false;
            disable();
        } // end if
        
    } // end usePIDOutput
    
    /**
     * Allows the Co-drier to control the arm via joystick input.
     * 
     * @param input The joystick value.
     */
    
    public void manualDrive(double input) {
        
        mainArmMotor.set(input);
        
    } // end driveArmManually
    
    /*
     * Constructor
     */
    
    public PIDMainArmSubsystem(String name, int mainArmVic, int mainPot) {
        super(name, Kp, Ki, Kd);

        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.
        
        armName = name;
        mainArmMotor = new Victor(mainArmVic);
        mainArmPot = new AnalogChannel(mainPot);
        
        setSetpointRange(MainArmPosition.MAIN_ARM_EXTENDED.value, 
                MainArmPosition.MAIN_ARM_FULLY_LOWERED.value);
        
        System.out.println(armName + " has been constructed.");
        
    } // end PIDMainArmSubsystem Constructor
    
} // end PIDMainArmSubsystem